
/*
 * adjust u fot v set,
 * 9ms per time
 */

void fit_angle(float v_set, float angle)
{

#define T 0.156f
#define L 0.1445f
#define K 50.0f / 9.0f //622.8f
    static float sero_du;
    float v1_set, v2_set;
    v1_set = v_set * (1.0f + T * fast_tan(angle) / 2.0f / L);
    v2_set = v_set * (1.0f - T * fast_tan(angle) / 2.0f / L);
    if (v1_set * v2_set < 0)
    {
        if (fabsf(v1_set) > fabsf(v2_set))
        {
            v1_set -= v2_set;
            v2_set = 0;
        }
        else
        {
            v2_set -= v1_set;
            v1_set = 0;
        }
    }
    if (angle > 0)
        angle *= 7 / 5.0f;
    sero_du = K * angle;
    // limit_to(&sero_du, sero_u_thre);
    // TA1CCR1 = sero_u_base + (int)sero_du;
#undef T
#undef L
#undef K
}

const float tan_table[500] = {-1.6331e+16, -158.98, -79.487, -52.988, -39.737, -31.786, -26.485,
                              -22.697, -19.856, -17.646, -15.877, -14.43, -13.223, -12.202,
                              -11.327, -10.567, -9.9028, -9.3162, -8.7946, -8.3276, -7.9071,
                              -7.5265, -7.1803, -6.864, -6.5739, -6.3068, -6.0601, -5.8315,
                              -5.6191, -5.4212, -5.2364, -5.0633, -4.9009, -4.7483, -4.6045,
                              -4.4687, -4.3404, -4.219, -4.1038, -3.9944, -3.8903, -3.7913,
                              -3.6968, -3.6067, -3.5205, -3.4381, -3.3592, -3.2835, -3.2109,
                              -3.1411, -3.0741, -3.0096, -2.9475, -2.8877, -2.83, -2.7743,
                              -2.7206, -2.6686, -2.6184, -2.5698, -2.5227, -2.4771, -2.4329,
                              -2.39, -2.3484, -2.3081, -2.2688, -2.2307, -2.1936, -2.1576,
                              -2.1225, -2.0883, -2.055, -2.0226, -1.991, -1.9601, -1.93,
                              -1.9007, -1.872, -1.844, -1.8166, -1.7899, -1.7638, -1.7382,
                              -1.7132, -1.6887, -1.6647, -1.6412, -1.6182, -1.5957, -1.5736,
                              -1.552, -1.5307, -1.5099, -1.4895, -1.4694, -1.4497, -1.4304,
                              -1.4114, -1.3927, -1.3744, -1.3564, -1.3387, -1.3213, -1.3041,
                              -1.2873, -1.2707, -1.2544, -1.2383, -1.2225, -1.2069, -1.1916,
                              -1.1765, -1.1616, -1.1469, -1.1325, -1.1182, -1.1042, -1.0903,
                              -1.0766, -1.0631, -1.0498, -1.0367, -1.0237, -1.0109, -0.9983,
                              -0.98582, -0.97349, -0.96131, -0.94928, -0.9374, -0.92565, -0.91404,
                              -0.90256, -0.89121, -0.87998, -0.86889, -0.85791, -0.84705, -0.8363,
                              -0.82567, -0.81514, -0.80473, -0.79442, -0.78421, -0.7741, -0.76409,
                              -0.75417, -0.74435, -0.73462, -0.72498, -0.71543, -0.70596, -0.69658,
                              -0.68728, -0.67806, -0.66891, -0.65985, -0.65086, -0.64194, -0.63309,
                              -0.62431, -0.61561, -0.60697, -0.59839, -0.58988, -0.58143, -0.57305,
                              -0.56472, -0.55646, -0.54825, -0.54009, -0.532, -0.52395, -0.51596,
                              -0.50802, -0.50014, -0.4923, -0.48451, -0.47676, -0.46907, -0.46142,
                              -0.45381, -0.44624, -0.43872, -0.43124, -0.4238, -0.4164, -0.40904,
                              -0.40172, -0.39443, -0.38718, -0.37996, -0.37278, -0.36564, -0.35852,
                              -0.35144, -0.34439, -0.33737, -0.33038, -0.32341, -0.31648, -0.30957,
                              -0.30269, -0.29584, -0.28901, -0.28221, -0.27543, -0.26867, -0.26194,
                              -0.25523, -0.24854, -0.24187, -0.23523, -0.2286, -0.22199, -0.2154,
                              -0.20882, -0.20227, -0.19573, -0.18921, -0.1827, -0.17621, -0.16973,
                              -0.16326, -0.15681, -0.15037, -0.14395, -0.13753, -0.13113, -0.12474,
                              -0.11835, -0.11198, -0.10562, -0.09926, -0.092912, -0.086572, -0.080238,
                              -0.07391, -0.067589, -0.061273, -0.054962, -0.048655, -0.042352, -0.036052,
                              -0.029755, -0.023461, -0.017168, -0.010877, -0.0045864, 0.0017037, 0.0079938,
                              0.014285, 0.020577, 0.02687, 0.033166, 0.039464, 0.045766, 0.052071,
                              0.05838, 0.064694, 0.071013, 0.077337, 0.083668, 0.090006, 0.09635,
                              0.1027, 0.10906, 0.11543, 0.12181, 0.1282, 0.1346, 0.14101,
                              0.14743, 0.15386, 0.16031, 0.16676, 0.17324, 0.17972, 0.18622,
                              0.19274, 0.19927, 0.20582, 0.21238, 0.21897, 0.22557, 0.23219,
                              0.23882, 0.24548, 0.25216, 0.25886, 0.26559, 0.27233, 0.2791,
                              0.28589, 0.29271, 0.29955, 0.30642, 0.31331, 0.32023, 0.32718,
                              0.33416, 0.34117, 0.3482, 0.35527, 0.36237, 0.3695, 0.37667,
                              0.38387, 0.3911, 0.39837, 0.40568, 0.41302, 0.42041, 0.42783,
                              0.43529, 0.44279, 0.45034, 0.45792, 0.46555, 0.47323, 0.48095,
                              0.48872, 0.49654, 0.5044, 0.51232, 0.52029, 0.5283, 0.53638,
                              0.5445, 0.55269, 0.56093, 0.56923, 0.57758, 0.586, 0.59448,
                              0.60303, 0.61164, 0.62032, 0.62906, 0.63787, 0.64676, 0.65572,
                              0.66475, 0.67386, 0.68304, 0.69231, 0.70165, 0.71108, 0.72059,
                              0.73019, 0.73988, 0.74966, 0.75953, 0.7695, 0.77956, 0.78972,
                              0.79999, 0.81036, 0.82083, 0.83141, 0.84211, 0.85291, 0.86384,
                              0.87488, 0.88605, 0.89734, 0.90876, 0.92031, 0.932, 0.94382,
                              0.95578, 0.96789, 0.98015, 0.99256, 1.0051, 1.0178, 1.0307,
                              1.0438, 1.057, 1.0704, 1.084, 1.0978, 1.1118, 1.1259,
                              1.1403, 1.1549, 1.1697, 1.1847, 1.1999, 1.2154, 1.2311,
                              1.247, 1.2632, 1.2797, 1.2964, 1.3134, 1.3307, 1.3482,
                              1.3661, 1.3843, 1.4028, 1.4216, 1.4408, 1.4603, 1.4802,
                              1.5005, 1.5211, 1.5422, 1.5636, 1.5855, 1.6078, 1.6306,
                              1.6539, 1.6776, 1.7019, 1.7266, 1.752, 1.7778, 1.8043,
                              1.8314, 1.8591, 1.8875, 1.9165, 1.9463, 1.9767, 2.008,
                              2.0401, 2.0729, 2.1067, 2.1414, 2.177, 2.2136, 2.2512,
                              2.2899, 2.3298, 2.3708, 2.4131, 2.4567, 2.5016, 2.548,
                              2.5959, 2.6454, 2.6965, 2.7495, 2.8043, 2.861, 2.9199,
                              2.9809, 3.0443, 3.1101, 3.1786, 3.2498, 3.3241, 3.4015,
                              3.4823, 3.5667, 3.655, 3.7474, 3.8443, 3.9461, 4.0529,
                              4.1654, 4.284, 4.4091, 4.5413, 4.6813, 4.8298, 4.9876,
                              5.1557, 5.335, 5.5267, 5.7323, 5.9532, 6.1914, 6.4488,
                              6.728, 7.0319, 7.3639, 7.7281, 8.1296, 8.5743, 9.0698,
                              9.6252, 10.252, 10.966, 11.785, 12.735, 13.851, 15.18,
                              16.789, 18.778, 21.301, 24.603, 29.115, 35.651, 45.964,
                              64.667, 109.02, 346.9};

float fast_tan(float theta)
{
    const float k = 25.0 / 9;
    int index;
    index = (int)(theta * k);
    index += 250;

    return tan_table[index];
}
